#ifndef SERVO_H
#define SERVO_H
#include <p18f4520.h>
#include "ieee_robot.h"

//function definitions for servo.c
//void write_timer0(unsigned short int);
//void write_timer1(unsigned short int);
/* servo macros and function prototypes */
#define write_timer0(byte)    TMR0H = (byte) >> 8;    \
                              TMR0L = (byte) & 0xFF
                              
#define write_timer1(byte)    TMR1H = (byte) >> 8;  \
                              TMR1L = (byte) & 0xFF
                              
void init_usart();

//defines for servo controller
#define set_position(x) servo_command[2] = x; \
     pos = -1; \
     PIE1 = PIE1 | 0x10

//Pololu cmd size
#define CMD_SIZE 3   
                                            
//macro defines for Timer 0 module
#define TMR0_INT_FLAG   (INTCON & 0x04)     //Timer0 interrupt flag
#define TMR1_INT_FLAG   (PIR1 & 0x01)       //Timer1 Interrupt Flag

//Value asigned to TMR0 ((65536 - 44799) * 1uS = 20ms (50Hz)
//with additional adjustment based on experimentation
#define TMR0_VALUE ((unsigned short int) 45700)

//to go straight need TMR1 to generate interrupts every
//1.406 ms by equation ((65536 - x) * 0.5uS = 1.406ms)
#define SERVO_STRAIGHT   ((unsigned short int) 64750)
#define SERVO_LEFT_90    ((unsigned short int) 64450)
#define SERVO_LEFT_45    ((unsigned short int) 64510)
#define SERVO_LEFT_35    ((unsigned short int) 64642)
#define SERVO_LEFT_23    ((unsigned short int) 64700)
#define SERVO_LEFT_15    ((unsigned short int) 64725)
//#define SERVO_LEFT_8     ((unsigned short int) 65100)
#define SERVO_RIGHT_45   ((unsigned short int) 65110)
#define SERVO_RIGHT_35   ((unsigned short int) 65025)
#define SERVO_RIGHT_23   ((unsigned short int) 64983)
#define SERVO_RIGHT_15   ((unsigned short int) 64840)
#define SERVO_RIGHT_8    ((unsigned short int) 64800)

#if SERVO_BACK == 1
//if servo is in back use these macros
	#if SERVO == TIMERS
	     #define Go_Straight()    servo_position = SERVO_STRAIGHT
	     #define Go_Left_90()     servo_position = SERVO_LEFT_90
	     #define Go_Left_45()     servo_position = SERVO_LEFT_45
		#define Go_Left_35()	servo_position = SERVO_LEFT_35
	     #define Go_Left_23()     servo_position = SERVO_LEFT_23
	     #define Go_Left_15()     servo_position = SERVO_LEFT_15
	     #define Go_Right_45()    servo_position = SERVO_RIGHT_45
	     #define Go_Right_23()    servo_position = SERVO_RIGHT_23
	#elif SERVO == CONTROLLER
	     #define Go_Straight()    set_position(0x00)
	     #define Go_Left_90()     set_position(0x00)
	     #define Go_Left_45()     set_position(0x00)
	     #define Go_Left_23()     set_position(0x00)
	     #define Go_Right_45()    set_position(0x00)
	     #define Go_Right_23()    set_position(0x00)
	#else
	     #warning "No way to turn robot, servo.h"
	#endif
#elif SERVO_BACK == 0
	#if SERVO == TIMERS
	     #define Go_Straight()    servo_position = SERVO_LEFT_35
	     #define Go_Right_90()    servo_position = SERVO_LEFT_90
	     #define Go_Right_45()    servo_position = SERVO_LEFT_45
		#define Go_Right_35()	servo_position = SERVO_LEFT_35
	     #define Go_Right_23()    servo_position = SERVO_LEFT_23
		#define Go_Left_90()	servo_position = SERVO_RIGHT_90
	     #define Go_Left_45()     servo_position = SERVO_RIGHT_45
		#define Go_Left_35()	servo_position = SERVO_RIGHT_35
	     #define Go_Left_23()     servo_position = SERVO_RIGHT_23
		#define Go_Left_15()     servo_position = SERVO_RIGHT_15
		#define Go_Left_8()      servo_position = SERVO_RIGHT_8
	#elif SERVO == CONTROLLER
	     #define Go_Straight()    set_position(0x00)
	     #define Go_Left_90()     set_position(0x00)
	     #define Go_Left_45()     set_position(0x00)
	     #define Go_Left_23()     set_position(0x00)
	     #define Go_Right_45()    set_position(0x00)
	     #define Go_Right_23()    set_position(0x00)
	#else
	     #warning "No way to turn robot, servo.h"
	#endif

#else
	#warning "No way to turn servo"
#endif //SERVO_BACK macros



#define set_servo_position()  if( (PORTB & 0x10) == 0 ) {	\
                                   Go_Left_35();            \
                              } else {	                    \
                                   if ( (PORTB & 0x70) == 0x70 ) { \
                                        Go_Straight();      \
                                   } else {                 \
                                        Go_Right_45();	     \
                                   }                        \
                              }
#endif //servo.h